#include "stdio.h"
#include "string.h"
#include "stdint.h"
#include "signal.h"
#include "unistd.h"
#include "stdlib.h"
#include "pthread.h"
#include "math.h"

// SOEM库头文件
#include "soem/soem.h"
#include "osal.h"
#include "oshw.h"
#include <inttypes.h>

// -------------------------- 全局配置（需根据实际硬件修改）--------------------------
#define MAX_SLAVES         25      // 最大支持电机数量
#define EC_CYCLETIME       2000000 // 2ms（单位：ns）
#define EC_CYCLETIME_US    (EC_CYCLETIME / 1000) // 转换为us
#define MODE_CSP            8

// -------------------------- 全局配置修改 --------------------------
#define MODE_PVT            5       // 力位混合模式（原CSP模式）


#define DEFAULT_RESOLUTION 131072  // 默认电机每转脉冲数
#define STATUS_PRINT_INTERVAL 2000 // 状态打印间隔（ms）
#define INFO_PRINT_ENABLE  1       // 调试信息开关（1=开启，0=关闭）

// -------------------------- 默认弧度配置（可被单个轴配置覆盖）--------------------------
#define DEFAULT_MAX_RADIANS        6.283185307  // 默认最大限位弧度（2π，即一圈）
#define DEFAULT_MIN_RADIANS        -6.283185307 // 默认最小限位弧度（-2π，即负一圈）
#define DEFAULT_SCAN_RADIANS       0.2  // 默认扫描轴对应往复弧度（π/8）
#define DEFAULT_SCAN_CYCLE_COUNT   2000         // 默认扫描周期数

// -------------------------- 6个轴的独立配置 --------------------------
typedef struct {
    double min_radians;      // 最小弧度限制
    double max_radians;      // 最大弧度限制  
    double scan_radians;     // 扫描弧度幅度
    int scan_cycle_count;    // 扫描周期数
    int resolution;          // 电机分辨率（每转脉冲数）
    char name[32];           // 轴名称
} axis_config_t;

// 6个轴的独立配置数组
axis_config_t axis_configs[6] = {
    // 轴0配置 - 131072分辨率
    { -6.283185307, 6.283185307, 0.2, 2000, 131072, "Axis-0" },
    // 轴1配置 - 262144分辨率  
    { -6.283185307, 6.283185307, 0.261799387, 2000, 131072, "Axis-1" },
    // 轴2配置 - 131072分辨率
    { -6.283185307, 6.283185307, 0.261799387, 2000, 131072, "Axis-2" },
    // 轴3配置 - 262144分辨率
    { -6.283185307, 6.283185307, 0.261799387, 2000, 131072, "Axis-3" },
    // 轴4配置 - 131072分辨率
    { -15.70796326, 15.70796326, 1, 2000, 262144, "Axis-4" },
    // 轴5配置 - 262144分辨率
    { -15.70796326, 15.70796326, 1, 2000, 262144, "Axis-5" }
};

// -------------------------- DS402协议关键定义 --------------------------
// 状态字位定义
#define STATUSWORD_MODE_READY  (1 << 8)  // 位8：模式就绪
#define STATUSWORD_FAULT       (1 << 3)  // 位3：故障状态
#define STATUSWORD_SWITCH_ON   (1 << 0)  // 位0：开关使能
#define STATUSWORD_OP_ENABLE   (1 << 1)  // 位1：操作使能
#define STATUSWORD_READY_SWITCH (1 << 2) // 位2：就绪切换
// 控制字定义（分阶段使用）
#define CW_POWER_OFF        0x0006  // 电源关闭（过渡状态）
#define CW_OPERATION_READY  0x000F  // 操作使能+模式确认（CSP激活核心）
// PDO映射相关索引
#define PDO_INPUT_MAP       0x1A00  // 输入PDO映射表索引
#define PDO_INPUT_MODE_SUB  0x0F    // 输入PDO第15字节（对应mode_op）
#define OBJECT_MODE_DISPLAY 0x6061  // DS402当前模式索引（需映射到PDO）

// -------------------------- 条件打印宏 --------------------------
#if INFO_PRINT_ENABLE
#define INFO_PRINT(...) printf(__VA_ARGS__)
#else
#define INFO_PRINT(...) do {} while(0)
#endif

// -------------------------- PDO映射结构体（1字节对齐，与从站严格匹配）--------------------------
#pragma pack(push, 1)
// 输出PDO（主站→从站：16字节）
typedef struct {
    uint16_t controlword;    // 控制字（0x6040:00）- 2字节
    int32_t  target_pos;     // 目标位置（0x607A:00）- 4字节
    int32_t  target_vel;     // 目标速度（0x60FF:00）- 4字节（CSP模式可忽略）
    int16_t  target_torque;  // 目标扭矩（0x6071:00）- 2字节（CSP模式可忽略）
    int16_t  max_torque;     // 最大扭矩限制（0x6072:00）- 2字节
    int8_t   mode_op;        // 控制模式（0x6060:00）- 1字节（强制CSP=0x08）
    int8_t   reserved;       // 预留字节 - 1字节
} outputs_t;

// 输入PDO（从站→主站：16字节）
typedef struct {
    uint16_t statusword;     // 状态字（0x6041:00）- 2字节
    int32_t  actual_pos;     // 实际位置（0x6064:00）- 4字节
    int32_t  actual_vel;     // 实际速度（0x606C:00）- 4字节
    int16_t  actual_torque;  // 实际扭矩（0x6077:00）- 2字节
    int16_t  error_code;     // 错误码（0x603F:00）- 2字节
    int8_t   mode_op;        // 当前控制模式（0x6061:00）- 1字节（需映射）
    int8_t   reserved;       // 预留字节 - 1字节
} inputs_t;
#pragma pack(pop)

// -------------------------- 电机配置结构体 --------------------------
typedef struct {
    int slave_index;         // 从站索引（1-based）
    uint8* outputs;          // 输出PDO地址指针
    uint8* inputs;           // 输入PDO地址指针
    int Obytes;              // 输出PDO字节数（需=16）
    int Ibytes;              // 输入PDO字节数（需=16）
    boolean is_operational;  // 从站是否OP状态
    // 控制参数
    int32_t target_pos;      // 目标位置
    int32_t actual_pos;      // 实际位置（从RPDO读取）
    int32_t base_pos_for_increment; // 第100周期基准位置（后续递增用）
    int cycle_cnt;           // 电机独立周期计数
    // 状态跟踪
    int fault_cleared;       // 故障标记（0=未清除，1=已清除）
    char name[32];           // 电机名称（如Motor-0）
    int last_error_code;     // 上一次错误码（避免重复打印）
    // 弧度控制参数（新增）
    double current_radians;  // 当前弧度值
    double target_radians;   // 目标弧度值
    // 轴独立配置参数（新增）
    double min_radians;      // 该轴最小弧度限制
    double max_radians;      // 该轴最大弧度限制
    double scan_radians;     // 该轴扫描弧度幅度
    int scan_cycle_count;    // 该轴扫描周期数
    int resolution;          // 该电机分辨率（每转脉冲数）
    double pulses_per_radian;// 每弧度对应的脉冲数
    char axis_name[32];      // 轴名称
} motor_config_t;

// -------------------------- 全局变量 --------------------------
ecx_contextt ecx_context;       // SOEM上下文
uint8_t      IOmap[4096];        // PDO映射缓存
pthread_t    thread_monitor;     // 从站状态监控线程
pthread_t    thread_status_printer; // 状态打印线程
static motor_config_t motor_configs[MAX_SLAVES]; // 电机配置数组
static int motor_count = 0;      // 实际配置电机数量
// 控制状态变量
volatile int  wkc;               // 工作计数器
int           expectedWKC;       // 预期工作计数器
boolean       inOP = FALSE;      // 主站是否OP状态
boolean       needlf = FALSE;    // 日志换行标记
uint8_t       current_group = 0; // 同步组（默认0）
volatile boolean exit_flag = FALSE; // 退出标志（Ctrl+C触发）
volatile int motor_csp_ready[MAX_SLAVES] = {0}; // 电机CSP就绪标志

// -------------------------- 函数声明 --------------------------
void *slave_monitor(void *ptr);
void *status_printer_thread(void *ptr);
void csp_control_main(char *ifname);
void print_usage();
void signal_handler(int sig);
// 电机控制核心函数
void init_motor_configs(void);
void setup_motor_control_parameters(int motor_idx);
void control_single_motor(int motor_idx);
void print_motor_status(int motor_idx);
// 诊断与辅助函数
void detailed_pdo_analysis(void);
void print_all_slave_status(void);
void print_ecat_rawdata_pdo(int global_cycle);
boolean validate_motor_data_access(int motor_idx);
const char* ec_state_to_string(uint16_t state);
// 弧度转换函数（新增）
double position_to_radians(int32_t position, int resolution);
int32_t radians_to_position(double radians, int resolution);
void apply_position_limits(int motor_idx);
// 轴配置函数（新增）
void print_axis_configurations(void);
void apply_axis_configuration(int motor_idx, int axis_index);

// -------------------------- 轴配置应用函数（新增）--------------------------
void apply_axis_configuration(int motor_idx, int axis_index) {
    if (axis_index < 0 || axis_index >= 6) {
        INFO_PRINT("WARNING: 轴索引%d超出范围，使用默认配置\n", axis_index);
        axis_index = 0;
    }
    
    motor_config_t *motor = &motor_configs[motor_idx];
    axis_config_t *axis = &axis_configs[axis_index];
    
    motor->min_radians = axis->min_radians;
    motor->max_radians = axis->max_radians;
    motor->scan_radians = axis->scan_radians;
    motor->scan_cycle_count = axis->scan_cycle_count;
    motor->resolution = axis->resolution;
    motor->pulses_per_radian = (double)axis->resolution / (2 * M_PI);
    strncpy(motor->axis_name, axis->name, 32);
    
    INFO_PRINT("  Motor %d 应用轴配置: %s\n", motor_idx, motor->axis_name);
    INFO_PRINT("    弧度范围: [%.6f, %.6f] rad\n", motor->min_radians, motor->max_radians);
    INFO_PRINT("    扫描幅度: %.6f rad, 周期数: %d\n", motor->scan_radians, motor->scan_cycle_count);
    INFO_PRINT("    电机分辨率: %d 脉冲/转\n", motor->resolution);
    INFO_PRINT("    每弧度脉冲: %.2f\n", motor->pulses_per_radian);
}

void print_axis_configurations(void) {
    INFO_PRINT("\n===================== 6轴独立配置信息 =====================\n");
    for (int i = 0; i < 6; i++) {
        double pulses_per_radian = (double)axis_configs[i].resolution / (2 * M_PI);
        INFO_PRINT("轴 %d (%s):\n", i, axis_configs[i].name);
        INFO_PRINT("  弧度范围: [%.6f, %.6f] rad\n", 
               axis_configs[i].min_radians, axis_configs[i].max_radians);
        INFO_PRINT("  扫描幅度: %.6f rad, 周期数: %d\n", 
               axis_configs[i].scan_radians, axis_configs[i].scan_cycle_count);
        INFO_PRINT("  电机分辨率: %d 脉冲/转\n", axis_configs[i].resolution);
        INFO_PRINT("  每弧度脉冲: %.2f, 总脉冲范围: [%d, %d]\n",
               pulses_per_radian,
               radians_to_position(axis_configs[i].min_radians, axis_configs[i].resolution),
               radians_to_position(axis_configs[i].max_radians, axis_configs[i].resolution));
    }
    INFO_PRINT("===========================================================\n");
}

// -------------------------- 弧度转换函数（支持不同分辨率）--------------------------
double position_to_radians(int32_t position, int resolution) {
    return (double)position / ((double)resolution / (2 * M_PI));
}

int32_t radians_to_position(double radians, int resolution) {
    return (int32_t)(radians * ((double)resolution / (2 * M_PI)));
}

void apply_position_limits(int motor_idx) {
    motor_config_t *motor = &motor_configs[motor_idx];
    
    // 转换为弧度检查限位
    double current_rad = position_to_radians(motor->target_pos, motor->resolution);
    
    if (current_rad > motor->max_radians) {
        motor->target_pos = radians_to_position(motor->max_radians, motor->resolution);
        INFO_PRINT("WARNING: %s (Motor %d) 超过最大弧度限制 %.6f，限制到 %.6f\n", 
               motor->axis_name, motor_idx, current_rad, motor->max_radians);
    } else if (current_rad < motor->min_radians) {
        motor->target_pos = radians_to_position(motor->min_radians, motor->resolution);
        INFO_PRINT("WARNING: %s (Motor %d) 超过最小弧度限制 %.6f，限制到 %.6f\n", 
               motor->axis_name, motor_idx, current_rad, motor->min_radians);
    }
}

// -------------------------- 从站状态转字符串（调试用）--------------------------
const char* ec_state_to_string(uint16_t state) {
    switch(state) {
        case EC_STATE_INIT: return "INIT";
        case EC_STATE_PRE_OP: return "PRE_OP";
        case EC_STATE_SAFE_OP: return "SAFE_OP";
        case EC_STATE_OPERATIONAL: return "OPERATIONAL";
        case EC_STATE_SAFE_OP + EC_STATE_ERROR: return "SAFE_OP+ERROR";
        case EC_STATE_NONE: return "NONE (Lost)";
        default: return "UNKNOWN";
    }
}

// -------------------------- 主函数 --------------------------
int main(int argc, char **argv) {
    signal(SIGINT, signal_handler);
    INFO_PRINT("===================== EtherCAT多电机CSP控制程序 =====================\n");
    
    // 打印轴配置信息
    print_axis_configurations();

    // 检查参数（需传入网卡名称）
    if (argc != 2) {
        print_usage();
        // 打印可用网卡
        ec_adaptert *adapter = ec_find_adapters();
        ec_adaptert *adapter_head = adapter;
        if (adapter != NULL) {
            INFO_PRINT("\n可用以太网适配器:\n");
            while (adapter != NULL) {
                INFO_PRINT("  - %s  (%s)\n", adapter->name, adapter->desc);
                adapter = adapter->next;
            }
        } else {
            INFO_PRINT("\n未发现可用以太网适配器!\n");
        }
        ec_free_adapters(adapter_head);
        return -1;
    }

    // 创建监控线程
    if (pthread_create(&thread_monitor, NULL, slave_monitor, NULL) != 0) {
        INFO_PRINT("ERROR: 创建从站监控线程失败!\n");
        return -1;
    }
    if (pthread_create(&thread_status_printer, NULL, status_printer_thread, NULL) != 0) {
        INFO_PRINT("ERROR: 创建状态打印线程失败!\n");
        pthread_join(thread_monitor, NULL);
        return -1;
    }

    // 启动CSP控制主逻辑
    csp_control_main(argv[1]);

    // 退出清理
    inOP = FALSE;
    pthread_join(thread_monitor, NULL);
    pthread_join(thread_status_printer, NULL);
    INFO_PRINT("\n===================== 程序正常退出 =====================\n");
    return 0;
}

// -------------------------- 从站状态监控线程（处理从站离线/错误）--------------------------
void *slave_monitor(void *ptr) {
    (void)ptr;
    int slave_idx;

    while (!exit_flag) {
        if (inOP && ((wkc < expectedWKC) || ecx_context.grouplist[current_group].docheckstate)) {
            if (needlf) {
                needlf = FALSE;
                INFO_PRINT("\n");
            }

            ecx_context.grouplist[current_group].docheckstate = FALSE;
            ecx_readstate(&ecx_context);

            // 遍历所有从站检查状态
            for (slave_idx = 1; slave_idx <= ecx_context.slavecount; slave_idx++) {
                ec_slavet *slave = &ecx_context.slavelist[slave_idx];
                if (slave->group != current_group) continue;

                // 从站非OP状态处理
                if (slave->state != EC_STATE_OPERATIONAL) {
                    ecx_context.grouplist[current_group].docheckstate = TRUE;
                    // 更新电机运行状态
                    for (int i = 0; i < motor_count; i++) {
                        if (motor_configs[i].slave_index == slave_idx) {
                            motor_configs[i].is_operational = FALSE;
                            break;
                        }
                    }

                    // 状态分支处理
                    if (slave->state == (EC_STATE_SAFE_OP + EC_STATE_ERROR)) {
                        INFO_PRINT("ERROR: Slave %d(%s) 状态: [SAFE_OP+ERROR]，监控603F错误码...\n", 
                                 slave_idx, motor_configs[slave_idx-1].axis_name);
                    } else if (slave->state == EC_STATE_SAFE_OP) {
                        INFO_PRINT("WARNING: Slave %d(%s) 状态: [SAFE_OP]，尝试切换到OP...\n", 
                                 slave_idx, motor_configs[slave_idx-1].axis_name);
                        slave->state = EC_STATE_OPERATIONAL;
                        ecx_writestate(&ecx_context, slave_idx);
                    } else if (slave->state > EC_STATE_NONE && ecx_reconfig_slave(&ecx_context, slave_idx, EC_TIMEOUTRET)) {
                        slave->islost = FALSE;
                        INFO_PRINT("INFO: Slave %d(%s) 重新配置成功!\n", slave_idx, motor_configs[slave_idx-1].axis_name);
                    } else if (!slave->islost) {
                        ecx_statecheck(&ecx_context, slave_idx, EC_STATE_OPERATIONAL, EC_TIMEOUTRET * 5);
                        if (slave->state == EC_STATE_NONE) {
                            slave->islost = TRUE;
                            INFO_PRINT("ERROR: Slave %d(%s) 丢失!\n", slave_idx, motor_configs[slave_idx-1].axis_name);
                        }
                    }
                }

                // 从站恢复检测
                if (slave->islost) {
                    if (slave->state == EC_STATE_NONE) {
                        if (ecx_recover_slave(&ecx_context, slave_idx, EC_TIMEOUTRET)) {
                            slave->islost = FALSE;
                            for (int i = 0; i < motor_count; i++) {
                                if (motor_configs[i].slave_index == slave_idx) {
                                    motor_configs[i].is_operational = TRUE;
                                    break;
                                }
                            }
                            INFO_PRINT("INFO: Slave %d(%s) 恢复成功!\n", slave_idx, motor_configs[slave_idx-1].axis_name);
                        }
                    } else {
                        slave->islost = FALSE;
                        for (int i = 0; i < motor_count; i++) {
                            if (motor_configs[i].slave_index == slave_idx) {
                                motor_configs[i].is_operational = TRUE;
                                break;
                            }
                        }
                        INFO_PRINT("INFO: Slave %d(%s) 重新发现!\n", slave_idx, motor_configs[slave_idx-1].axis_name);
                    }
                }
            }

            // 所有从站恢复OP状态
            if (!ecx_context.grouplist[current_group].docheckstate) {
                INFO_PRINT("INFO: 所有从站恢复 [OPERATIONAL] 状态!\n");
                for (int i = 0; i < motor_count; i++) {
                    motor_configs[i].is_operational = TRUE;
                }
            }
        }
        osal_usleep(10000); // 10ms检查一次
    }
    return NULL;
}

// -------------------------- 状态打印线程（每2秒打印全量状态）--------------------------
void *status_printer_thread(void *ptr) {
    (void)ptr;
    while (!exit_flag) {
        osal_usleep(STATUS_PRINT_INTERVAL * 1000);
        if (ecx_context.slavecount > 0) {
            print_all_slave_status();
        }
    }
    return NULL;
}

// -------------------------- 全量状态打印（含从站+电机DS402状态）--------------------------
void print_all_slave_status(void) {
    ecx_readstate(&ecx_context);
    ecx_statecheck(&ecx_context, 0, EC_STATE_OPERATIONAL, 0);
    
    INFO_PRINT("\n===================== 从站状态快照 =====================\n");
    // 打印时间戳
    struct timeval tv;
    gettimeofday(&tv, NULL);
    uint64_t timestamp_ms = (uint64_t)tv.tv_sec * 1000 + tv.tv_usec / 1000;
    INFO_PRINT("时间戳: %lu ms\n", (unsigned long)timestamp_ms);
    
    // 检查所有从站是否OP
    int all_op = 1;
    for (int i = 1; i <= ecx_context.slavecount; i++) {
        if (ecx_context.slavelist[i].state != EC_STATE_OPERATIONAL) {
            all_op = 0;
            break;
        }
    }
    INFO_PRINT("总从站数: %d | 全部OP: %s\n", 
           ecx_context.slavecount, all_op ? "YES" : "NO");
    INFO_PRINT("-----------------------------------------------------------------\n");
    INFO_PRINT("%-6s %-20s %-15s %-8s %-10s %s\n", 
           "Slave", "轴名称", "状态", "WKC", "是否丢失", "AL错误码");
    INFO_PRINT("-----------------------------------------------------------------\n");
    
    // 打印每个从站基础信息
    for (int i = 1; i <= ecx_context.slavecount; i++) {
        ec_slavet *slave = &ecx_context.slavelist[i];
        const char* axis_name = (i-1 < motor_count) ? motor_configs[i-1].axis_name : "Unknown";
        INFO_PRINT("%-6d %-20s %-15s %-8d %-10s 0x%04X\n",
               i,
               axis_name,
               ec_state_to_string(slave->state),
               wkc,
               slave->islost ? "YES" : "NO",
               slave->ALstatuscode);
    }
    
    // 打印电机DS402状态
    INFO_PRINT("-----------------------------------------------------------------\n");
    INFO_PRINT("电机DS402状态汇总:\n");
    for (int i = 0; i < motor_count; i++) {
        inputs_t *in_ptr = (inputs_t *)motor_configs[i].inputs;
        int mode_ready = 0, has_fault = 0;
        uint16_t error_code = 0;
        if (in_ptr) {
            mode_ready = (in_ptr->statusword & STATUSWORD_MODE_READY) ? 1 : 0;
            has_fault = (in_ptr->statusword & STATUSWORD_FAULT) ? 1 : 0;
            error_code = in_ptr->error_code;
            
            // 错误码变化时打印（避免重复）
            if (error_code != motor_configs[i].last_error_code) {
                INFO_PRINT("  ALERT: %s (Motor %d) 603F错误码变化 - 旧:0x%04X → 新:0x%04X\n",
                       motor_configs[i].axis_name, i, motor_configs[i].last_error_code, error_code);
                motor_configs[i].last_error_code = error_code;
            }
        }
        
        // 计算当前弧度
        double current_rad = position_to_radians(motor_configs[i].actual_pos, motor_configs[i].resolution);
        double target_rad = position_to_radians(motor_configs[i].target_pos, motor_configs[i].resolution);
        
        INFO_PRINT("  %s (Motor %d):\n", motor_configs[i].axis_name, i);
        INFO_PRINT("    OP=%-3s | 故障=%-1d | 模式就绪=%-1d | 当前模式=0x%02X\n",
               motor_configs[i].is_operational ? "YES" : "NO",
               has_fault,
               mode_ready,
               in_ptr ? in_ptr->mode_op : 0xFF);
        INFO_PRINT("    位置=%8d(%.4frad) | 目标=%8d(%.4frad)\n",
               motor_configs[i].actual_pos, current_rad,
               motor_configs[i].target_pos, target_rad);
        INFO_PRINT("    状态字=0x%04X | 错误码=0x%04X | 周期=%d\n",
               in_ptr ? in_ptr->statusword : 0xFFFF,
               error_code,
               motor_configs[i].cycle_cnt);
        INFO_PRINT("    配置: 范围[%.3f,%.3f]rad | 扫描%.3frad/%d周期 | 分辨率:%d\n",
               motor_configs[i].min_radians, motor_configs[i].max_radians,
               motor_configs[i].scan_radians, motor_configs[i].scan_cycle_count,
               motor_configs[i].resolution);
    }
    INFO_PRINT("=================================================================\n\n");
}

// -------------------------- PDO详细分析（调试用，确认映射地址/大小）--------------------------
void detailed_pdo_analysis(void) {
    INFO_PRINT("\n=== PDO详细分析 ===\n");
    // 主站总映射大小
    INFO_PRINT("主站PDO总大小:\n");
    INFO_PRINT("  输出: %d bytes | 输入: %d bytes\n", 
           ecx_context.slavelist[0].Obytes, ecx_context.slavelist[0].Ibytes);
    // 各从站单独大小
    INFO_PRINT("各从站PDO大小:\n");
    for (int i = 1; i <= ecx_context.slavecount; i++) {
        ec_slavet *slave = &ecx_context.slavelist[i];
        INFO_PRINT("  Slave %d (%s):\n", i, slave->name);
        INFO_PRINT("    输出: %d位(%d字节) | 输入: %d位(%d字节)\n", 
               slave->Obits, slave->Obytes, slave->Ibits, slave->Ibytes);
        // 同步管理器信息
        for (int sm = 2; sm <= 3; sm++) {
            if (slave->SM[sm].StartAddr > 0) {
                INFO_PRINT("    SM%d: 起始地址=0x%04X | 长度=%d字节 | 类型=%d\n", 
                       sm, slave->SM[sm].StartAddr, slave->SM[sm].SMlength, slave->SMtype[sm]);
            }
        }
    }
    // 计算总大小差异
    int total_out = 0, total_in = 0;
    for (int i = 1; i <= ecx_context.slavecount; i++) {
        total_out += ecx_context.slavelist[i].Obytes;
        total_in += ecx_context.slavelist[i].Ibytes;
    }
    INFO_PRINT("大小校验:\n");
    INFO_PRINT("  输出总和: %d bytes (主站总大小: %d bytes | 差异: %d bytes)\n",
           total_out, ecx_context.slavelist[0].Obytes, ecx_context.slavelist[0].Obytes - total_out);
    INFO_PRINT("  输入总和: %d bytes (主站总大小: %d bytes | 差异: %d bytes)\n",
           total_in, ecx_context.slavelist[0].Ibytes, ecx_context.slavelist[0].Ibytes - total_in);
    // 地址偏移检查（避免重叠）
    INFO_PRINT("\nPDO地址偏移:\n");
    for (int i = 1; i <= ecx_context.slavecount; i++) {
        ec_slavet *slave = &ecx_context.slavelist[i];
        INFO_PRINT("  Slave %d:\n", i);
        INFO_PRINT("    输出偏移: %ld | 输入偏移: %ld\n", 
               slave->outputs - IOmap, slave->inputs - IOmap);
        // 检查与前一个从站是否重叠
        if (i > 1) {
            ec_slavet *prev = &ecx_context.slavelist[i-1];
            int out_gap = slave->outputs - (prev->outputs + prev->Obytes);
            int in_gap = slave->inputs - (prev->inputs + prev->Ibytes);
            if (out_gap < 0) INFO_PRINT("    WARNING: 输出PDO与Slave %d重叠（间隙:%d字节）\n", i-1, out_gap);
            if (in_gap < 0) INFO_PRINT("    WARNING: 输入PDO与Slave %d重叠（间隙:%d字节）\n", i-1, in_gap);
        }
    }
}

// -------------------------- ECAT RawData打印（每20周期打印PDO原始字节）--------------------------
void print_ecat_rawdata_pdo(int global_cycle) {
    INFO_PRINT("\n===================== ECAT PDO RawData（周期: %d） =====================\n", global_cycle);
    for (int motor_idx = 0; motor_idx < motor_count; motor_idx++) {
        // 静默检查，不打印警告
        if (motor_configs[motor_idx].outputs == NULL || 
            motor_configs[motor_idx].inputs == NULL) {
            continue; // 静默跳过无效电机
        }
        
        motor_config_t *motor = &motor_configs[motor_idx];
        // 打印输出PDO（TPDO）
        INFO_PRINT("%s (Motor %d, Slave %d) | TPDO（输出）: ", motor->axis_name, motor_idx, motor->slave_index);
        for (int i = 0; i < motor->Obytes; i++) {
            INFO_PRINT("%02X ", motor->outputs[i]);
        }
        INFO_PRINT("| 控制字=0x%04X | 目标位置=%d | 模式=0x%02X\n",
               ((outputs_t *)motor->outputs)->controlword,
               ((outputs_t *)motor->outputs)->target_pos,
               ((outputs_t *)motor->outputs)->mode_op);
        // 打印输入PDO（RPDO）
        INFO_PRINT("%s (Motor %d, Slave %d) | RPDO（输入）: ", motor->axis_name, motor_idx, motor->slave_index);
        for (int i = 0; i < motor->Ibytes; i++) {
            INFO_PRINT("%02X ", motor->inputs[i]);
        }
        INFO_PRINT("| 状态字=0x%04X | 实际位置=%d | 当前模式=0x%02X\n",
               ((inputs_t *)motor->inputs)->statusword,
               ((inputs_t *)motor->inputs)->actual_pos,
               ((inputs_t *)motor->inputs)->mode_op);
    }
    INFO_PRINT("=======================================================================\n");
}

// -------------------------- 电机数据访问验证（避免空指针/大小不足）--------------------------
boolean validate_motor_data_access(int motor_idx) {
    if (motor_idx < 0 || motor_idx >= motor_count) {
        return FALSE;
    }
    if (motor_configs[motor_idx].outputs == NULL || motor_configs[motor_idx].inputs == NULL) {
        INFO_PRINT("WARNING: %s (Motor %d) PDO指针为空!\n", motor_configs[motor_idx].axis_name, motor_idx);
        return FALSE;
    }
    if (motor_configs[motor_idx].Obytes < sizeof(outputs_t) || 
        motor_configs[motor_idx].Ibytes < sizeof(inputs_t)) {
        return FALSE;
    }
    return TRUE;
}

// -------------------------- 初始化电机配置（遍历从站，绑定PDO地址）--------------------------
void init_motor_configs(void) {
    motor_count = 0;
    // 遍历所有从站，初始化电机配置
    for (int i = 1; i <= ecx_context.slavecount && motor_count < MAX_SLAVES; i++) {

        // 检查从站是否有有效的PDO映射
        if (ecx_context.slavelist[i].outputs == NULL || ecx_context.slavelist[i].inputs == NULL) {
            INFO_PRINT("INFO: Slave %d 无有效PDO映射，不作为电机设备\n", i);
            continue; // 跳过这个从站
        }

        // 检查PDO大小是否足够
        if (ecx_context.slavelist[i].Obytes < sizeof(outputs_t) || 
            ecx_context.slavelist[i].Ibytes < sizeof(inputs_t)) {
            INFO_PRINT("INFO: Slave %d PDO大小不足，不作为电机设备 (需O:%lu/I:%lu，实际O:%d/I:%d)\n", 
                   i, sizeof(outputs_t), sizeof(inputs_t),
                   ecx_context.slavelist[i].Obytes, ecx_context.slavelist[i].Ibytes);
            continue; // 跳过这个从站
        }
        
        motor_config_t *motor = &motor_configs[motor_count];
        // 绑定从站信息
        motor->slave_index = i;
        motor->Obytes = ecx_context.slavelist[i].Obytes;
        motor->Ibytes = ecx_context.slavelist[i].Ibytes;
        motor->outputs = ecx_context.slavelist[i].outputs;
        motor->inputs = ecx_context.slavelist[i].inputs;
        motor->is_operational = FALSE;
        // 初始化状态变量
        motor->fault_cleared = 0;
        motor->last_error_code = 0;
        motor->target_pos = 0;
        motor->actual_pos = 0;
        motor->base_pos_for_increment = 0;
        motor->cycle_cnt = 0;
        // 初始化弧度参数
        motor->current_radians = 0.0;
        motor->target_radians = 0.0;
        motor_csp_ready[motor_count] = 0;
        
        // 应用轴配置（按顺序分配，第0个电机用轴0配置，第1个用轴1配置，以此类推）
        int axis_index = motor_count % 6; // 循环使用6个轴配置
        apply_axis_configuration(motor_count, axis_index);
        
        // 初始化控制参数（SDO配置CSP、PDO映射等）
        setup_motor_control_parameters(motor_count);
        // 命名电机
        snprintf(motor->name, 32, "Motor-%d", motor_count);
        // 打印初始化信息
        INFO_PRINT("  %s (Motor %d): Slave %d | 输出地址=0x%lx | 输入地址=0x%lx | O:%d字节 | I:%d字节\n", 
               motor->axis_name, motor_count, i, 
               (unsigned long)motor->outputs, (unsigned long)motor->inputs,
               motor->Obytes, motor->Ibytes);
        motor_count++;
    }
    INFO_PRINT("INFO: 共配置 %d 台电机\n", motor_count);
}

// -------------------------- 电机控制参数配置（核心优化：SDO+CSP+PDO映射）--------------------------
void setup_motor_control_parameters(int motor_idx) {
    motor_config_t *motor = &motor_configs[motor_idx];
    int slave_idx = motor->slave_index;
    int ret = 0;

    // 先读取当前PDO映射配置进行检查
    uint32_t current_mapping = 0;
    int size_current_mapping = sizeof(current_mapping);  // 修复：使用int变量
    ret = ecx_SDOread(&ecx_context, slave_idx, PDO_INPUT_MAP, PDO_INPUT_MODE_SUB, FALSE, 
                     &size_current_mapping, &current_mapping, EC_TIMEOUTRXM);  // 修复：传递int指针
    if (ret == 1) {
        INFO_PRINT("INFO: %s (Motor %d) 当前PDO映射[0x%02X]: 0x%08X\n", 
               motor->axis_name, motor_idx, PDO_INPUT_MODE_SUB, current_mapping);
    }

    // 如果当前映射不是我们需要的，才进行配置
    uint32_t desired_mapping = (OBJECT_MODE_DISPLAY << 16) | (0x00 << 8) | 0x08;
    if (current_mapping != desired_mapping) {
        // 先设置映射数量为0（禁用映射）
        uint8_t zero_count = 0;
        int size_zero_count = sizeof(zero_count);  // 修复：使用int变量
        ret = ecx_SDOwrite(&ecx_context, slave_idx, PDO_INPUT_MAP, 0x00, FALSE, 
                          size_zero_count, &zero_count, EC_TIMEOUTRXM);  // 修复：传递int值
        osal_usleep(10000); // 等待10ms
        
        // 配置模式显示对象映射
        int size_desired_mapping = sizeof(desired_mapping);  // 修复：使用int变量
        ret = ecx_SDOwrite(&ecx_context, slave_idx, PDO_INPUT_MAP, PDO_INPUT_MODE_SUB, FALSE, 
                          size_desired_mapping, &desired_mapping, EC_TIMEOUTRXM);  // 修复：传递int值
        if (ret == 1) {
            INFO_PRINT("SUCCESS: %s (Motor %d, Slave %d) 输入PDO第15字节映射到0x6061:00成功\n", 
                   motor->axis_name, motor_idx, slave_idx);
        } else {
            INFO_PRINT("WARNING: %s (Motor %d, Slave %d) PDO映射配置失败，使用默认映射\n", 
                   motor->axis_name, motor_idx, slave_idx);
        }

        // 重新激活映射
        uint8_t pdo_count = PDO_INPUT_MODE_SUB;
        int size_pdo_count = sizeof(pdo_count);  // 修复：使用int变量
        ret = ecx_SDOwrite(&ecx_context, slave_idx, PDO_INPUT_MAP, 0x00, FALSE, 
                          size_pdo_count, &pdo_count, EC_TIMEOUTRXM);  // 修复：传递int值
        if (ret == 1) {
            INFO_PRINT("INFO: %s (Motor %d, Slave %d) 输入PDO映射激活成功\n", motor->axis_name, motor_idx, slave_idx);
        }
    } else {
        INFO_PRINT("INFO: %s (Motor %d, Slave %d) PDO映射已正确配置\n", motor->axis_name, motor_idx, slave_idx);
    }

    // -------------------------- SDO强制写入力位混合模式（0x6060:00=0x05）--------------------------
    int8_t csv_mode = MODE_PVT;
    int size_csv_mode = sizeof(csv_mode);
    ret = ecx_SDOwrite(&ecx_context, slave_idx, 0x6060, 0x00, FALSE, 
                      size_csv_mode, &csv_mode, EC_TIMEOUTRXM);
    if (ret == 1) {
        INFO_PRINT("SUCCESS: %s (Motor %d, Slave %d) SDO配置0x6060:00=力位混合模式(0x05)成功\n", 
               motor->axis_name, motor_idx, slave_idx);
    } else {
        INFO_PRINT("ALERT: %s (Motor %d, Slave %d) SDO配置力位混合模式失败！\n", motor->axis_name, motor_idx, slave_idx);
    }

    // -------------------------- 设置PVT_KP参数（索引2000:00=20）--------------------------
    int32_t pvt_kp_value = 0;
    int size_pvt_kp = sizeof(pvt_kp_value);
    ret = ecx_SDOwrite(&ecx_context, slave_idx, 0x2000, 0x00, FALSE, 
                      size_pvt_kp, &pvt_kp_value, EC_TIMEOUTRXM);
    if (ret == 1) {
        INFO_PRINT("SUCCESS: %s (Motor %d, Slave %d) SDO配置PVT_KP(0x2000:00)=%d成功\n", 
               motor->axis_name, motor_idx, slave_idx, pvt_kp_value);
    } else {
        INFO_PRINT("WARNING: %s (Motor %d, Slave %d) SDO配置PVT_KP失败\n", motor->axis_name, motor_idx, slave_idx);
    }

    // -------------------------- SDO初始化控制字（电源关闭状态）--------------------------
    uint16_t init_cw = 0x0006; // Shutdown
    int size_init_cw = sizeof(init_cw);  // 修复：使用int变量
    ret = ecx_SDOwrite(&ecx_context, slave_idx, 0x6040, 0x00, FALSE, 
                      size_init_cw, &init_cw, EC_TIMEOUTRXM);  // 修复：传递int值
    if (ret == 1) {
        INFO_PRINT("INFO: %s (Motor %d, Slave %d) SDO初始化控制字=0x%04X成功\n", 
               motor->axis_name, motor_idx, slave_idx, init_cw);
    }

    // 设置最大扭矩（保护电机，可根据实际调整）
    int16_t max_torque = 50;
    int size_max_torque = sizeof(max_torque);  // 修复：使用int变量
    ret = ecx_SDOwrite(&ecx_context, slave_idx, 0x6072, 0x00, FALSE, 
                      size_max_torque, &max_torque, EC_TIMEOUTRXM);  // 修复：传递int值
    if (ret == 1) {
        INFO_PRINT("INFO: %s (Motor %d, Slave %d) SDO设置最大扭矩=%d\n", motor->axis_name, motor_idx, slave_idx, max_torque);
    }
}

// -------------------------- 单电机CSP控制（基于独立轴配置的往复运动）--------------------------
// void control_single_motor(int motor_idx) {
//     // 静默检查，不打印警告
//     if (motor_configs[motor_idx].outputs == NULL || 
//         motor_configs[motor_idx].inputs == NULL) {
//         return; // 静默返回，不控制无效电机
//     }

//     motor_config_t *motor = &motor_configs[motor_idx];
//     outputs_t *out_ptr = (outputs_t *)motor->outputs;
//     inputs_t  *in_ptr = (inputs_t *)motor->inputs;

//     // 1. 基础指令配置（确保持续发送CSP模式）
//     out_ptr->mode_op = MODE_CSP;          // 强制CSP模式（0x08）
//     out_ptr->max_torque = 1000;           // 最大扭矩限制

//     // 电机独立周期计数
//     motor->cycle_cnt++;
//     int current_cycle = motor->cycle_cnt;

//     // 2. 状态解析（故障+错误码监控）
//     uint16_t status = in_ptr ? in_ptr->statusword : 0x0000;
//     uint16_t error_code = in_ptr ? in_ptr->error_code : 0x0000;
//     int has_fault = (status & STATUSWORD_FAULT) ? 1 : 0;

//     // 错误码变化时打印
//     if (has_fault && in_ptr && error_code != motor->last_error_code) {
//         INFO_PRINT("ALERT: %s (Motor %d, 周期:%d) - 故障状态(0x%04X) | 603F错误码:0x%04X\n",
//                motor->axis_name, motor_idx, current_cycle, status, error_code);
//         motor->last_error_code = error_code;
        
//         // 故障时发送故障复位
//         out_ptr->controlword = 0x0080; // 故障复位
//         motor->fault_cleared = 0; // 重置故障清除标志
//         return;
//     }

//     // 3. 目标位置计算（基于独立轴配置的往复运动）
//     if (in_ptr != NULL) {
//         motor->actual_pos = in_ptr->actual_pos;
//         motor->current_radians = position_to_radians(motor->actual_pos, motor->resolution);
        
//         // 前100周期：记录基准位置
//         if (current_cycle <= 100) {
//             motor->target_pos = motor->actual_pos;
//             motor->target_radians = motor->current_radians;
            
//             if (current_cycle == 100) {
//                 motor->base_pos_for_increment = motor->actual_pos;
//                 INFO_PRINT("INFO: %s (Motor %d) - 第100周期，记录基准位置: %d (%.6frad)\n",
//                        motor->axis_name, motor_idx, motor->base_pos_for_increment, motor->current_radians);
//             }
//         } else {
//             // 计算相对于基准位置的周期数
//             int relative_cycle = current_cycle - 100;
            
//             // 使用该电机独立的扫描周期数
//             int scan_cycle_count = motor->scan_cycle_count;
//             double scan_radians = motor->scan_radians;
            
//             // 每scan_cycle_count个周期为一个完整的往复循环
//             int cycle_in_loop = relative_cycle % scan_cycle_count;
            
//             // 计算当前在往复运动中的弧度位置
//             if (cycle_in_loop < (scan_cycle_count / 2)) {
//                 // 前一半周期：正向运动，从0到scan_radians
//                 motor->target_radians = (double)cycle_in_loop / (scan_cycle_count / 2) * scan_radians;
//             } else {
//                 // 后一半周期：反向运动，从scan_radians回到0
//                 motor->target_radians = scan_radians - (double)(cycle_in_loop - scan_cycle_count/2) / (scan_cycle_count/2) * scan_radians;
//             }
            
//             // 转换为目标位置（使用该电机的分辨率）
//             int32_t base_radians_pos = radians_to_position(position_to_radians(motor->base_pos_for_increment, motor->resolution), motor->resolution);
//             motor->target_pos = base_radians_pos + radians_to_position(motor->target_radians, motor->resolution);
            
//             // 应用该电机独立的位置限位
//             apply_position_limits(motor_idx);
            
//             // 在关键点打印状态信息
//             if (cycle_in_loop == 0) {
//                 INFO_PRINT("%s (Motor %d) - 周期:%d, 开始新的往复循环，目标弧度: %.6f\n",
//                        motor->axis_name, motor_idx, current_cycle, motor->target_radians);
//             } else if (cycle_in_loop == (scan_cycle_count / 2)) {
//                 INFO_PRINT("%s (Motor %d) - 周期:%d, 达到正向峰值，目标弧度: %.6f\n",
//                        motor->axis_name, motor_idx, current_cycle, motor->target_radians);
//             } else if (cycle_in_loop == scan_cycle_count - 1) {
//                 INFO_PRINT("%s (Motor %d) - 周期:%d, 完成往复循环，回到起点，目标弧度: %.6f\n",
//                        motor->axis_name, motor_idx, current_cycle, motor->target_radians);
//             }
            
//             // 每该轴扫描周期数的1/8打印一次位置信息
//             if (cycle_in_loop % (scan_cycle_count / 8) == 0) {
//                 double current_offset_rad = motor->target_radians;
//                 INFO_PRINT("%s (Motor %d) - 周期:%d, 弧度偏移: %.6f/%.6f (%.1f%%), 目标位置: %d\n",
//                        motor->axis_name, motor_idx, current_cycle, current_offset_rad, scan_radians, 
//                        (current_offset_rad / scan_radians) * 100.0, motor->target_pos);
//             }
//         }
//     } else {
//         // 备用逻辑：如果无法读取实际位置
//         if (current_cycle == 100) {
//             motor->base_pos_for_increment = 0;
//             INFO_PRINT("WARNING: %s (Motor %d) - 无法读取实际位置，基准位置设为0\n", motor->axis_name, motor_idx);
//         } else if (current_cycle > 100) {
//             int relative_cycle = current_cycle - 100;
//             int scan_cycle_count = motor->scan_cycle_count;
//             double scan_radians = motor->scan_radians;
//             int cycle_in_loop = relative_cycle % scan_cycle_count;
            
//             // 计算目标弧度（与主逻辑相同）
//             if (cycle_in_loop < (scan_cycle_count / 2)) {
//                 motor->target_radians = (double)cycle_in_loop / (scan_cycle_count / 2) * scan_radians;
//             } else {
//                 motor->target_radians = scan_radians - (double)(cycle_in_loop - scan_cycle_count/2) / (scan_cycle_count/2) * scan_radians;
//             }
            
//             motor->target_pos = radians_to_position(motor->target_radians, motor->resolution);
//             apply_position_limits(motor_idx);
//         }
//     }
    
//     out_ptr->target_pos = motor->target_pos;

//     // -------------------------- 增强的DS402标准控制字使能序列 --------------------------
//     if (!motor->fault_cleared) {
//         // 阶段0：故障处理（无故障则标记清除）
//         out_ptr->controlword = 0x0006; // Shutdown
//         if (!has_fault) {
//             motor->fault_cleared = 1;
//             INFO_PRINT("INFO: %s (Motor %d) - 无故障，标记故障已清除 (周期:%d)\n",
//                    motor->axis_name, motor_idx, current_cycle);
//         }
//     } else if (!motor_csp_ready[motor_idx]) {
//         // 阶段1：完整的DS402状态机切换
//         if (current_cycle < 100) {
//             // 步骤1：Shutdown (0x0006) - 稳定100周期
//             out_ptr->controlword = 0x0006;
//             if (current_cycle == 1) {
//                 INFO_PRINT("INFO: %s (Motor %d) - 步骤1/4：Shutdown (周期:%d)\n", motor->axis_name, motor_idx, current_cycle);
//             }
//         } else if (current_cycle < 200) {
//             // 步骤2：Switch on (0x0007) - 稳定100周期
//             out_ptr->controlword = 0x0007;
//             if (current_cycle == 100) {
//                 INFO_PRINT("INFO: %s (Motor %d) - 步骤2/4：Switch on (控制字:0x0007, 周期:%d)\n",
//                        motor->axis_name, motor_idx, current_cycle);
//             }
//         } else if (current_cycle < 300) {
//             // 步骤3：Enable operation (0x000F) - 稳定100周期
//             out_ptr->controlword = 0x000F;
//             if (current_cycle == 200) {
//                 INFO_PRINT("INFO: %s (Motor %d) - 步骤3/4：Enable operation (控制字:0x000F, 周期:%d)\n",
//                        motor->axis_name, motor_idx, current_cycle);
//             }
//         } else {
//             // 步骤4：持续操作使能，等待CSP模式激活
//             out_ptr->controlword = 0x000F;
            
//             // 检查CSP模式激活条件
//             if (in_ptr) {
//                 uint16_t status = in_ptr->statusword;
//                 int mode_ready = (status & STATUSWORD_MODE_READY) ? 1 : 0;
//                 int switch_on = (status & STATUSWORD_SWITCH_ON) ? 1 : 0;
//                 int op_enabled = (status & STATUSWORD_OP_ENABLE) ? 1 : 0;
//                 int current_mode = in_ptr->mode_op;
                
//                 // CSP模式激活条件：模式就绪+操作使能+当前模式=CSP
//                 if (mode_ready && op_enabled && current_mode == MODE_CSP) {
//                     motor_csp_ready[motor_idx] = 1;
//                     INFO_PRINT("SUCCESS: %s (Motor %d) - CSP模式激活成功! 状态字=0x%04X 模式=0x%02X (周期:%d)\n",
//                            motor->axis_name, motor_idx, status, current_mode, current_cycle);
//                 }
                
//                 // 每200周期打印一次等待状态
//                 if (current_cycle % 200 == 0) {
//                     INFO_PRINT("%s (Motor %d) 等待CSP: 状态字=0x%04X(RS=%d,SO=%d,OE=%d) 模式=0x%02X 周期=%d\n",
//                            motor->axis_name, motor_idx, status, 
//                            (status & STATUSWORD_READY_SWITCH) ? 1 : 0,
//                            switch_on, op_enabled,
//                            current_mode, current_cycle);
//                 }
//             }
//         }
//     } else {
//         // 阶段2：CSP运行中，持续发送操作使能指令
//         out_ptr->controlword = 0x000F;
//     }
// }
// -------------------------- 单电机CSP控制（基于独立轴配置的往复运动+停顿）--------------------------
// void control_single_motor(int motor_idx) {
//     // 静默检查，不打印警告
//     if (motor_configs[motor_idx].outputs == NULL || 
//         motor_configs[motor_idx].inputs == NULL) {
//         return; // 静默返回，不控制无效电机
//     }

//     motor_config_t *motor = &motor_configs[motor_idx];
//     outputs_t *out_ptr = (outputs_t *)motor->outputs;
//     inputs_t  *in_ptr = (inputs_t *)motor->inputs;

//     // 1. 基础指令配置（确保持续发送CSP模式）
//     out_ptr->mode_op = MODE_CSP;          // 强制CSP模式（0x08）
//     out_ptr->max_torque = 1000;           // 最大扭矩限制

//     // 电机独立周期计数
//     motor->cycle_cnt++;
//     int current_cycle = motor->cycle_cnt;

//     // 2. 状态解析（故障+错误码监控）
//     uint16_t status = in_ptr ? in_ptr->statusword : 0x0000;
//     uint16_t error_code = in_ptr ? in_ptr->error_code : 0x0000;
//     int has_fault = (status & STATUSWORD_FAULT) ? 1 : 0;

//     // 错误码变化时打印
//     if (has_fault && in_ptr && error_code != motor->last_error_code) {
//         INFO_PRINT("ALERT: %s (Motor %d, 周期:%d) - 故障状态(0x%04X) | 603F错误码:0x%04X\n",
//                motor->axis_name, motor_idx, current_cycle, status, error_code);
//         motor->last_error_code = error_code;
        
//         // 故障时发送故障复位
//         out_ptr->controlword = 0x0080; // 故障复位
//         motor->fault_cleared = 0; // 重置故障清除标志
//         return;
//     }

//     // 3. 检查是否处于停顿状态
//     static int pause_start_cycle[MAX_SLAVES] = {0};
//     static int in_pause[MAX_SLAVES] = {0};
    
//     if (in_pause[motor_idx]) {
//         // 计算停顿时间（500个周期 = 500 * 2ms = 1000ms = 1s）
//         if (current_cycle - pause_start_cycle[motor_idx] >= 500) {
//             in_pause[motor_idx] = 0; // 结束停顿
//             INFO_PRINT("INFO: %s (Motor %d) - 1秒停顿结束，开始新的往复循环 (周期:%d)\n",
//                    motor->axis_name, motor_idx, current_cycle);
//         } else {
//             // 在停顿期间保持当前位置不变
//             out_ptr->target_pos = motor->target_pos;
            
//             // 继续发送操作使能指令
//             out_ptr->controlword = 0x000F;
//             return; // 跳过正常的位置计算
//         }
//     }

//     // 4. 目标位置计算（基于独立轴配置的往复运动）
//     if (in_ptr != NULL) {
//         motor->actual_pos = in_ptr->actual_pos;
//         motor->current_radians = position_to_radians(motor->actual_pos, motor->resolution);
        
//         // 前100周期：记录基准位置
//         if (current_cycle <= 100) {
//             motor->target_pos = motor->actual_pos;
//             motor->target_radians = motor->current_radians;
            
//             if (current_cycle == 100) {
//                 motor->base_pos_for_increment = motor->actual_pos;
//                 INFO_PRINT("INFO: %s (Motor %d) - 第100周期，记录基准位置: %d (%.6frad)\n",
//                        motor->axis_name, motor_idx, motor->base_pos_for_increment, motor->current_radians);
//             }
//         } else {
//             // 计算相对于基准位置的周期数
//             int relative_cycle = current_cycle - 100;
            
//             // 使用该电机独立的扫描周期数
//             int scan_cycle_count = motor->scan_cycle_count;
//             double scan_radians = motor->scan_radians;
            
//             // 每scan_cycle_count个周期为一个完整的往复循环
//             int cycle_in_loop = relative_cycle % scan_cycle_count;
            
//             // 计算当前在往复运动中的弧度位置
//             if (cycle_in_loop < (scan_cycle_count / 2)) {
//                 // 前一半周期：正向运动，从0到scan_radians
//                 motor->target_radians = (double)cycle_in_loop / (scan_cycle_count / 2) * scan_radians;
//             } else {
//                 // 后一半周期：反向运动，从scan_radians回到0
//                 motor->target_radians = scan_radians - (double)(cycle_in_loop - scan_cycle_count/2) / (scan_cycle_count/2) * scan_radians;
//             }
            
//             // 转换为目标位置（使用该电机的分辨率）
//             int32_t base_radians_pos = radians_to_position(position_to_radians(motor->base_pos_for_increment, motor->resolution), motor->resolution);
//             motor->target_pos = base_radians_pos + radians_to_position(motor->target_radians, motor->resolution);
            
//             // 应用该电机独立的位置限位
//             apply_position_limits(motor_idx);
            
//             // 在关键点打印状态信息
//             if (cycle_in_loop == 0) {
//                 INFO_PRINT("%s (Motor %d) - 周期:%d, 开始新的往复循环，目标弧度: %.6f\n",
//                        motor->axis_name, motor_idx, current_cycle, motor->target_radians);
//             } else if (cycle_in_loop == (scan_cycle_count / 2)) {
//                 INFO_PRINT("%s (Motor %d) - 周期:%d, 达到正向峰值，目标弧度: %.6f\n",
//                        motor->axis_name, motor_idx, current_cycle, motor->target_radians);
//             } else if (cycle_in_loop == scan_cycle_count - 1) {
//                 // 完成一个完整循环，开始1秒停顿
//                 INFO_PRINT("%s (Motor %d) - 周期:%d, 完成往复循环，回到起点，开始1秒停顿\n",
//                        motor->axis_name, motor_idx, current_cycle);
//                 in_pause[motor_idx] = 1;
//                 pause_start_cycle[motor_idx] = current_cycle;
                
//                 // 确保目标位置设置为基准位置（0弧度）
//                 motor->target_radians = 0.0;
//                 motor->target_pos = base_radians_pos;
//             }
            
//             // 每该轴扫描周期数的1/8打印一次位置信息（不在停顿时）
//             if (!in_pause[motor_idx] && cycle_in_loop % (scan_cycle_count / 8) == 0) {
//                 double current_offset_rad = motor->target_radians;
//                 INFO_PRINT("%s (Motor %d) - 周期:%d, 弧度偏移: %.6f/%.6f (%.1f%%), 目标位置: %d\n",
//                        motor->axis_name, motor_idx, current_cycle, current_offset_rad, scan_radians, 
//                        (current_offset_rad / scan_radians) * 100.0, motor->target_pos);
//             }
//         }
//     } else {
//         // 备用逻辑：如果无法读取实际位置
//         if (current_cycle == 100) {
//             motor->base_pos_for_increment = 0;
//             INFO_PRINT("WARNING: %s (Motor %d) - 无法读取实际位置，基准位置设为0\n", motor->axis_name, motor_idx);
//         } else if (current_cycle > 100) {
//             int relative_cycle = current_cycle - 100;
//             int scan_cycle_count = motor->scan_cycle_count;
//             double scan_radians = motor->scan_radians;
//             int cycle_in_loop = relative_cycle % scan_cycle_count;
            
//             // 计算目标弧度（与主逻辑相同）
//             if (cycle_in_loop < (scan_cycle_count / 2)) {
//                 motor->target_radians = (double)cycle_in_loop / (scan_cycle_count / 2) * scan_radians;
//             } else {
//                 motor->target_radians = scan_radians - (double)(cycle_in_loop - scan_cycle_count/2) / (scan_cycle_count/2) * scan_radians;
//             }
            
//             motor->target_pos = radians_to_position(motor->target_radians, motor->resolution);
//             apply_position_limits(motor_idx);
            
//             // 检查是否需要开始停顿
//             if (cycle_in_loop == scan_cycle_count - 1) {
//                 INFO_PRINT("%s (Motor %d) - 周期:%d, 完成往复循环，开始1秒停顿\n",
//                        motor->axis_name, motor_idx, current_cycle);
//                 in_pause[motor_idx] = 1;
//                 pause_start_cycle[motor_idx] = current_cycle;
//                 motor->target_radians = 0.0;
//                 motor->target_pos = motor->base_pos_for_increment;
//             }
//         }
//     }
    
//     out_ptr->target_pos = motor->target_pos;

//     // -------------------------- 增强的DS402标准控制字使能序列 --------------------------
//     if (!motor->fault_cleared) {
//         // 阶段0：故障处理（无故障则标记清除）
//         out_ptr->controlword = 0x0006; // Shutdown
//         if (!has_fault) {
//             motor->fault_cleared = 1;
//             INFO_PRINT("INFO: %s (Motor %d) - 无故障，标记故障已清除 (周期:%d)\n",
//                    motor->axis_name, motor_idx, current_cycle);
//         }
//     } else if (!motor_csp_ready[motor_idx]) {
//         // 阶段1：完整的DS402状态机切换
//         if (current_cycle < 100) {
//             // 步骤1：Shutdown (0x0006) - 稳定100周期
//             out_ptr->controlword = 0x0006;
//             if (current_cycle == 1) {
//                 INFO_PRINT("INFO: %s (Motor %d) - 步骤1/4：Shutdown (周期:%d)\n", motor->axis_name, motor_idx, current_cycle);
//             }
//         } else if (current_cycle < 200) {
//             // 步骤2：Switch on (0x0007) - 稳定100周期
//             out_ptr->controlword = 0x0007;
//             if (current_cycle == 100) {
//                 INFO_PRINT("INFO: %s (Motor %d) - 步骤2/4：Switch on (控制字:0x0007, 周期:%d)\n",
//                        motor->axis_name, motor_idx, current_cycle);
//             }
//         } else if (current_cycle < 300) {
//             // 步骤3：Enable operation (0x000F) - 稳定100周期
//             out_ptr->controlword = 0x000F;
//             if (current_cycle == 200) {
//                 INFO_PRINT("INFO: %s (Motor %d) - 步骤3/4：Enable operation (控制字:0x000F, 周期:%d)\n",
//                        motor->axis_name, motor_idx, current_cycle);
//             }
//         } else {
//             // 步骤4：持续操作使能，等待CSP模式激活
//             out_ptr->controlword = 0x000F;
            
//             // 检查CSP模式激活条件
//             if (in_ptr) {
//                 uint16_t status = in_ptr->statusword;
//                 int mode_ready = (status & STATUSWORD_MODE_READY) ? 1 : 0;
//                 int switch_on = (status & STATUSWORD_SWITCH_ON) ? 1 : 0;
//                 int op_enabled = (status & STATUSWORD_OP_ENABLE) ? 1 : 0;
//                 int current_mode = in_ptr->mode_op;
                
//                 // CSP模式激活条件：模式就绪+操作使能+当前模式=CSP
//                 if (mode_ready && op_enabled && current_mode == MODE_CSP) {
//                     motor_csp_ready[motor_idx] = 1;
//                     INFO_PRINT("SUCCESS: %s (Motor %d) - CSP模式激活成功! 状态字=0x%04X 模式=0x%02X (周期:%d)\n",
//                            motor->axis_name, motor_idx, status, current_mode, current_cycle);
//                 }
                
//                 // 每200周期打印一次等待状态
//                 if (current_cycle % 200 == 0) {
//                     INFO_PRINT("%s (Motor %d) 等待CSP: 状态字=0x%04X(RS=%d,SO=%d,OE=%d) 模式=0x%02X 周期=%d\n",
//                            motor->axis_name, motor_idx, status, 
//                            (status & STATUSWORD_READY_SWITCH) ? 1 : 0,
//                            switch_on, op_enabled,
//                            current_mode, current_cycle);
//                 }
//             }
//         }
//     } else {
//         // 阶段2：CSP运行中，持续发送操作使能指令
//         out_ptr->controlword = 0x000F;
//     }
// }


// -------------------------- 单电机PVT控制（扭矩循环变化，位置保持不变）--------------------------
void control_single_motor(int motor_idx) {
    // 静默检查，不打印警告
    if (motor_configs[motor_idx].outputs == NULL || 
        motor_configs[motor_idx].inputs == NULL) {
        return; // 静默返回，不控制无效电机
    }

    motor_config_t *motor = &motor_configs[motor_idx];
    outputs_t *out_ptr = (outputs_t *)motor->outputs;
    inputs_t  *in_ptr = (inputs_t *)motor->inputs;

    // 1. 基础指令配置（力位混合模式）
    out_ptr->mode_op = MODE_PVT;          // 力位混合模式（0x05）
    out_ptr->max_torque = 100;           // 最大扭矩限制设置为100，确保覆盖±50范围

    // 电机独立周期计数
    motor->cycle_cnt++;
    int current_cycle = motor->cycle_cnt;

    // 2. 状态解析（故障+错误码监控）
    uint16_t status = in_ptr ? in_ptr->statusword : 0x0000;
    uint16_t error_code = in_ptr ? in_ptr->error_code : 0x0000;
    int has_fault = (status & STATUSWORD_FAULT) ? 1 : 0;

    // 错误码变化时打印
    if (has_fault && in_ptr && error_code != motor->last_error_code) {
        INFO_PRINT("ALERT: %s (Motor %d, 周期:%d) - 故障状态(0x%04X) | 603F错误码:0x%04X\n",
               motor->axis_name, motor_idx, current_cycle, status, error_code);
        motor->last_error_code = error_code;
        
        // 故障时发送故障复位
        out_ptr->controlword = 0x0080; // 故障复位
        motor->fault_cleared = 0; // 重置故障清除标志
        motor_csp_ready[motor_idx] = 0; // 重置就绪标志
        return;
    }

    // 3. 目标位置保持不变（使用实际位置或初始位置）
    if (in_ptr != NULL) {
        motor->actual_pos = in_ptr->actual_pos;
        motor->current_radians = position_to_radians(motor->actual_pos, motor->resolution);
        
        // 前100周期：记录基准位置，并保持目标位置不变
        if (current_cycle <= 100) {
            motor->target_pos = motor->actual_pos;
            motor->target_radians = motor->current_radians;
            
            if (current_cycle == 100) {
                motor->base_pos_for_increment = motor->actual_pos;
                INFO_PRINT("INFO: %s (Motor %d) - 第100周期，记录基准位置: %d (%.6frad)，位置将保持不变\n",
                       motor->axis_name, motor_idx, motor->base_pos_for_increment, motor->current_radians);
            }
        } else {
            // 保持目标位置不变
            motor->target_pos = motor->base_pos_for_increment;
            motor->target_radians = position_to_radians(motor->base_pos_for_increment, motor->resolution);
        }
    } else {
        // 如果无法读取实际位置，使用0位置
        if (current_cycle == 100) {
            motor->base_pos_for_increment = 0;
            INFO_PRINT("WARNING: %s (Motor %d) - 无法读取实际位置，基准位置设为0\n", motor->axis_name, motor_idx);
        }
        motor->target_pos = motor->base_pos_for_increment;
        motor->target_radians = position_to_radians(motor->base_pos_for_increment, motor->resolution);
    }
    
    // 设置目标位置（保持不变）
    out_ptr->target_pos = motor->target_pos;

    // 4. 扭矩循环控制逻辑
    static int torque_direction[MAX_SLAVES] = {0}; // 0: 减小, 1: 增加
    static int16_t current_torque[MAX_SLAVES] = {50}; // 初始扭矩为50
    
    // 计算当前扭矩值
    if (current_cycle > 100) { // 从第101周期开始扭矩变化
        if (torque_direction[motor_idx] == 0) {
            // 减小方向：从50减到-50
            current_torque[motor_idx]--;
            if (current_torque[motor_idx] <= -50) {
                current_torque[motor_idx] = -50;
                torque_direction[motor_idx] = 1; // 切换为增加方向
                INFO_PRINT("INFO: %s (Motor %d) - 扭矩达到最小值-50，开始增加 (周期:%d)\n",
                       motor->axis_name, motor_idx, current_cycle);
            }
        } else {
            // 增加方向：从-50增加到50
            current_torque[motor_idx]++;
            if (current_torque[motor_idx] >= 50) {
                current_torque[motor_idx] = 50;
                torque_direction[motor_idx] = 0; // 切换为减小方向
                INFO_PRINT("INFO: %s (Motor %d) - 扭矩达到最大值50，开始减小 (周期:%d)\n",
                       motor->axis_name, motor_idx, current_cycle);
            }
        }
        
        // 设置目标扭矩
        out_ptr->target_torque = current_torque[motor_idx];
        
        // 每50周期打印一次扭矩状态
        if (current_cycle % 50 == 0) {
            INFO_PRINT("%s (Motor %d) - 周期:%d, 目标扭矩: %d, 方向: %s\n",
                   motor->axis_name, motor_idx, current_cycle, 
                   current_torque[motor_idx],
                   torque_direction[motor_idx] == 0 ? "减小" : "增加");
        }
        
        // 在扭矩方向切换时打印详细信息
        if (current_torque[motor_idx] == 0) {
            INFO_PRINT("%s (Motor %d) - 周期:%d, 扭矩过零，当前值: %d\n",
                   motor->axis_name, motor_idx, current_cycle, current_torque[motor_idx]);
        }
    } else {
        // 前100周期使用初始扭矩50
        out_ptr->target_torque = 50;
    }

    // -------------------------- 简化的DS402控制字管理 --------------------------
    // 如果电机已经就绪，直接发送操作使能指令
    if (motor_csp_ready[motor_idx]) {
        out_ptr->controlword = 0x000F; // 持续操作使能
    } 
    // 如果还未就绪，执行完整的使能序列
    else if (!motor->fault_cleared) {
        // 阶段0：故障处理（无故障则标记清除）
        out_ptr->controlword = 0x0006; // Shutdown
        if (!has_fault) {
            motor->fault_cleared = 1;
            INFO_PRINT("INFO: %s (Motor %d) - 无故障，标记故障已清除 (周期:%d)\n",
                   motor->axis_name, motor_idx, current_cycle);
        }
    } else {
        // 阶段1：完整的DS402状态机切换
        if (current_cycle < 100) {
            // 步骤1：Shutdown (0x0006) - 稳定100周期
            out_ptr->controlword = 0x0006;
            if (current_cycle == 1) {
                INFO_PRINT("INFO: %s (Motor %d) - 步骤1/4：Shutdown (周期:%d)\n", motor->axis_name, motor_idx, current_cycle);
            }
        } else if (current_cycle < 200) {
            // 步骤2：Switch on (0x0007) - 稳定100周期
            out_ptr->controlword = 0x0007;
            if (current_cycle == 100) {
                INFO_PRINT("INFO: %s (Motor %d) - 步骤2/4：Switch on (控制字:0x0007, 周期:%d)\n",
                       motor->axis_name, motor_idx, current_cycle);
            }
        } else if (current_cycle < 300) {
            // 步骤3：Enable operation (0x000F) - 稳定100周期
            out_ptr->controlword = 0x000F;
            if (current_cycle == 200) {
                INFO_PRINT("INFO: %s (Motor %d) - 步骤3/4：Enable operation (控制字:0x000F, 周期:%d)\n",
                       motor->axis_name, motor_idx, current_cycle);
            }
        } else {
            // 步骤4：持续操作使能，等待模式激活
            out_ptr->controlword = 0x000F;
            
            // 检查模式激活条件
            if (in_ptr) {
                uint16_t status = in_ptr->statusword;
                int mode_ready = (status & STATUSWORD_MODE_READY) ? 1 : 0;
                int switch_on = (status & STATUSWORD_SWITCH_ON) ? 1 : 0;
                int op_enabled = (status & STATUSWORD_OP_ENABLE) ? 1 : 0;
                int current_mode = in_ptr->mode_op;
                
                // PVT模式激活条件：模式就绪+操作使能+当前模式=PVT
                if (mode_ready && op_enabled && current_mode == MODE_PVT) {
                    motor_csp_ready[motor_idx] = 1;
                    INFO_PRINT("SUCCESS: %s (Motor %d) - PVT模式激活成功! 状态字=0x%04X 模式=0x%02X (周期:%d)\n",
                           motor->axis_name, motor_idx, status, current_mode, current_cycle);
                }
                
                // 每200周期打印一次等待状态
                if (current_cycle % 200 == 0) {
                    INFO_PRINT("%s (Motor %d) 等待PVT: 状态字=0x%04X(RS=%d,SO=%d,OE=%d) 模式=0x%02X 周期=%d\n",
                           motor->axis_name, motor_idx, status, 
                           (status & STATUSWORD_READY_SWITCH) ? 1 : 0,
                           switch_on, op_enabled,
                           current_mode, current_cycle);
                }
            }
        }
    }
}
// -------------------------- 单电机CSP控制（基于独立轴配置的往复运动+停顿，保持使能）--------------------------
// void control_single_motor(int motor_idx) {
//     // 静默检查，不打印警告
//     if (motor_configs[motor_idx].outputs == NULL || 
//         motor_configs[motor_idx].inputs == NULL) {
//         return; // 静默返回，不控制无效电机
//     }

//     motor_config_t *motor = &motor_configs[motor_idx];
//     outputs_t *out_ptr = (outputs_t *)motor->outputs;
//     inputs_t  *in_ptr = (inputs_t *)motor->inputs;


//     // 1. 基础指令配置（力位混合模式）
//     out_ptr->mode_op = MODE_PVT;          // 力位混合模式（0x05）
//     out_ptr->max_torque = 100;           // 最大扭矩限制

//     // out_ptr->target_torque = -50;        // 目标扭矩（6071）设置为1000


//     // 电机独立周期计数
//     motor->cycle_cnt++;
//     int current_cycle = motor->cycle_cnt;

//     // 2. 状态解析（故障+错误码监控）
//     uint16_t status = in_ptr ? in_ptr->statusword : 0x0000;
//     uint16_t error_code = in_ptr ? in_ptr->error_code : 0x0000;
//     int has_fault = (status & STATUSWORD_FAULT) ? 1 : 0;

//     // 错误码变化时打印
//     if (has_fault && in_ptr && error_code != motor->last_error_code) {
//         INFO_PRINT("ALERT: %s (Motor %d, 周期:%d) - 故障状态(0x%04X) | 603F错误码:0x%04X\n",
//                motor->axis_name, motor_idx, current_cycle, status, error_code);
//         motor->last_error_code = error_code;
        
//         // 故障时发送故障复位
//         out_ptr->controlword = 0x0080; // 故障复位
//         motor->fault_cleared = 0; // 重置故障清除标志
//         motor_csp_ready[motor_idx] = 0; // 重置CSP就绪标志
//         return;
//     }

//     // 3. 检查是否处于停顿状态
//     static int pause_start_cycle[MAX_SLAVES] = {0};
//     static int in_pause[MAX_SLAVES] = {0};
    
//     if (in_pause[motor_idx]) {
//         // 计算停顿时间（500个周期 = 500 * 2ms = 1000ms = 1s）
//         if (current_cycle - pause_start_cycle[motor_idx] >= 500) {
//             in_pause[motor_idx] = 0; // 结束停顿
//             INFO_PRINT("INFO: %s (Motor %d) - 1秒停顿结束，开始新的往复循环 (周期:%d)\n",
//                    motor->axis_name, motor_idx, current_cycle);
//         } else {
//             // 在停顿期间保持当前位置不变，但保持操作使能状态
//             out_ptr->target_pos = motor->target_pos;
//             out_ptr->target_torque = 1000; // 保持目标扭矩
//             out_ptr->controlword = 0x000F; // 保持操作使能
            
//             // 计算剩余停顿时间（用于调试信息）
//             int remaining_cycles = 500 - (current_cycle - pause_start_cycle[motor_idx]);
//             if (remaining_cycles % 100 == 0) { // 每200ms打印一次
//                 INFO_PRINT("%s (Motor %d) - 停顿中，剩余 %.1f秒 (周期:%d)\n",
//                        motor->axis_name, motor_idx, (double)remaining_cycles * 0.002, current_cycle);
//             }
//             return; // 跳过正常的位置计算和使能序列
//         }
//     }

//     // 4. 目标位置计算（基于独立轴配置的往复运动）
//     if (in_ptr != NULL) {
//         motor->actual_pos = in_ptr->actual_pos;
//         motor->current_radians = position_to_radians(motor->actual_pos, motor->resolution);
        
//         // 前100周期：记录基准位置
//         if (current_cycle <= 100) {
//             motor->target_pos = motor->actual_pos;
//             motor->target_radians = motor->current_radians;
            
//             if (current_cycle == 100) {
//                 motor->base_pos_for_increment = motor->actual_pos;
//                 INFO_PRINT("INFO: %s (Motor %d) - 第100周期，记录基准位置: %d (%.6frad)\n",
//                        motor->axis_name, motor_idx, motor->base_pos_for_increment, motor->current_radians);
//             }
//         } else {
//             // 计算相对于基准位置的周期数
//             int relative_cycle = current_cycle - 100;
            
//             // 使用该电机独立的扫描周期数
//             int scan_cycle_count = motor->scan_cycle_count;
//           // 关键修改：扫描弧度幅度减少十倍
//             double scan_radians = motor->scan_radians * 0.1;  // 减少十倍
            
//             // 每scan_cycle_count个周期为一个完整的往复循环
//             int cycle_in_loop = relative_cycle % scan_cycle_count;
            
//             // 计算当前在往复运动中的弧度位置
//             if (cycle_in_loop < (scan_cycle_count / 2)) {
//                 // 前一半周期：正向运动，从0到scan_radians
//                 motor->target_radians = (double)cycle_in_loop / (scan_cycle_count / 2) * scan_radians;
//             } else {
//                 // 后一半周期：反向运动，从scan_radians回到0
//                 motor->target_radians = scan_radians - (double)(cycle_in_loop - scan_cycle_count/2) / (scan_cycle_count/2) * scan_radians;
//             }
            
//             // 转换为目标位置（使用该电机的分辨率）
//             int32_t base_radians_pos = radians_to_position(position_to_radians(motor->base_pos_for_increment, motor->resolution), motor->resolution);
//             motor->target_pos = base_radians_pos + radians_to_position(motor->target_radians, motor->resolution);
            
//             // 应用该电机独立的位置限位
//             apply_position_limits(motor_idx);
            
//             // 在关键点打印状态信息
//             if (cycle_in_loop == 0) {
//                 INFO_PRINT("%s (Motor %d) - 周期:%d, 开始新的往复循环，目标弧度: %.6f\n",
//                        motor->axis_name, motor_idx, current_cycle, motor->target_radians);
//             } else if (cycle_in_loop == (scan_cycle_count / 2)) {
//                 INFO_PRINT("%s (Motor %d) - 周期:%d, 达到正向峰值，目标弧度: %.6f\n",
//                        motor->axis_name, motor_idx, current_cycle, motor->target_radians);
//             } else if (cycle_in_loop == scan_cycle_count - 1) {
//                 // 完成一个完整循环，开始1秒停顿
//                 INFO_PRINT("%s (Motor %d) - 周期:%d, 完成往复循环，回到起点，开始1秒停顿\n",
//                        motor->axis_name, motor_idx, current_cycle);
//                 in_pause[motor_idx] = 1;
//                 pause_start_cycle[motor_idx] = current_cycle;
                
//                 // 确保目标位置设置为基准位置（0弧度）
//                 motor->target_radians = 0.0;
//                 motor->target_pos = base_radians_pos;
//             }
            
//             // 每该轴扫描周期数的1/8打印一次位置信息（不在停顿时）
//             if (!in_pause[motor_idx] && cycle_in_loop % (scan_cycle_count / 8) == 0) {
//                 double current_offset_rad = motor->target_radians;
//                 INFO_PRINT("%s (Motor %d) - 周期:%d, 弧度偏移: %.6f/%.6f (%.1f%%), 目标位置: %d\n",
//                        motor->axis_name, motor_idx, current_cycle, current_offset_rad, scan_radians, 
//                        (current_offset_rad / scan_radians) * 100.0, motor->target_pos);
//             }
//         }
//     } else {
//         // 备用逻辑：如果无法读取实际位置
//         if (current_cycle == 100) {
//             motor->base_pos_for_increment = 0;
//             INFO_PRINT("WARNING: %s (Motor %d) - 无法读取实际位置，基准位置设为0\n", motor->axis_name, motor_idx);
//         } else if (current_cycle > 100) {
//             int relative_cycle = current_cycle - 100;
//             int scan_cycle_count = motor->scan_cycle_count;
//             // double scan_radians = motor->scan_radians;
//                       // 关键修改：扫描弧度幅度减少十倍
//             double scan_radians = motor->scan_radians * 0.1;  // 减少十倍
//             int cycle_in_loop = relative_cycle % scan_cycle_count;
            
//             // 计算目标弧度（与主逻辑相同）
//             if (cycle_in_loop < (scan_cycle_count / 2)) {
//                 motor->target_radians = (double)cycle_in_loop / (scan_cycle_count / 2) * scan_radians;
//             } else {
//                 motor->target_radians = scan_radians - (double)(cycle_in_loop - scan_cycle_count/2) / (scan_cycle_count/2) * scan_radians;
//             }
            
//             motor->target_pos = radians_to_position(motor->target_radians, motor->resolution);
//             apply_position_limits(motor_idx);
            
//             // 检查是否需要开始停顿
//             if (cycle_in_loop == scan_cycle_count - 1) {
//                 INFO_PRINT("%s (Motor %d) - 周期:%d, 完成往复循环，开始1秒停顿\n",
//                        motor->axis_name, motor_idx, current_cycle);
//                 in_pause[motor_idx] = 1;
//                 pause_start_cycle[motor_idx] = current_cycle;
//                 motor->target_radians = 0.0;
//                 motor->target_pos = motor->base_pos_for_increment;
//             }
//         }
//     }
    
//     out_ptr->target_pos = motor->target_pos;

//     // -------------------------- 简化的DS402控制字管理 --------------------------
//     // 如果电机已经CSP就绪，直接发送操作使能指令
//     if (motor_csp_ready[motor_idx]) {
//         out_ptr->controlword = 0x000F; // 持续操作使能
//     } 
//     // 如果还未CSP就绪，执行完整的使能序列
//     else if (!motor->fault_cleared) {
//         // 阶段0：故障处理（无故障则标记清除）
//         out_ptr->controlword = 0x0006; // Shutdown
//         if (!has_fault) {
//             motor->fault_cleared = 1;
//             INFO_PRINT("INFO: %s (Motor %d) - 无故障，标记故障已清除 (周期:%d)\n",
//                    motor->axis_name, motor_idx, current_cycle);
//         }
//     } else {
//         // 阶段1：完整的DS402状态机切换
//         if (current_cycle < 100) {
//             // 步骤1：Shutdown (0x0006) - 稳定100周期
//             out_ptr->controlword = 0x0006;
//             if (current_cycle == 1) {
//                 INFO_PRINT("INFO: %s (Motor %d) - 步骤1/4：Shutdown (周期:%d)\n", motor->axis_name, motor_idx, current_cycle);
//             }
//         } else if (current_cycle < 200) {
//             // 步骤2：Switch on (0x0007) - 稳定100周期
//             out_ptr->controlword = 0x0007;
//             if (current_cycle == 100) {
//                 INFO_PRINT("INFO: %s (Motor %d) - 步骤2/4：Switch on (控制字:0x0007, 周期:%d)\n",
//                        motor->axis_name, motor_idx, current_cycle);
//             }
//         } else if (current_cycle < 300) {
//             // 步骤3：Enable operation (0x000F) - 稳定100周期
//             out_ptr->controlword = 0x000F;
//             if (current_cycle == 200) {
//                 INFO_PRINT("INFO: %s (Motor %d) - 步骤3/4：Enable operation (控制字:0x000F, 周期:%d)\n",
//                        motor->axis_name, motor_idx, current_cycle);
//             }
//         } else {
//             // 步骤4：持续操作使能，等待CSP模式激活
//             out_ptr->controlword = 0x000F;
            
//             // 检查CSP模式激活条件
//             if (in_ptr) {
//                 uint16_t status = in_ptr->statusword;
//                 int mode_ready = (status & STATUSWORD_MODE_READY) ? 1 : 0;
//                 int switch_on = (status & STATUSWORD_SWITCH_ON) ? 1 : 0;
//                 int op_enabled = (status & STATUSWORD_OP_ENABLE) ? 1 : 0;
//                 int current_mode = in_ptr->mode_op;
                
//                 // CSP模式激活条件：模式就绪+操作使能+当前模式=CSP
//                 if (mode_ready && op_enabled && current_mode == MODE_CSP) {
//                     motor_csp_ready[motor_idx] = 1;
//                     INFO_PRINT("SUCCESS: %s (Motor %d) - CSP模式激活成功! 状态字=0x%04X 模式=0x%02X (周期:%d)\n",
//                            motor->axis_name, motor_idx, status, current_mode, current_cycle);
//                 }
                
//                 // 每200周期打印一次等待状态
//                 if (current_cycle % 200 == 0) {
//                     INFO_PRINT("%s (Motor %d) 等待CSP: 状态字=0x%04X(RS=%d,SO=%d,OE=%d) 模式=0x%02X 周期=%d\n",
//                            motor->axis_name, motor_idx, status, 
//                            (status & STATUSWORD_READY_SWITCH) ? 1 : 0,
//                            switch_on, op_enabled,
//                            current_mode, current_cycle);
//                 }
//             }
//         }
//     }
// }
// -------------------------- 电机状态简印（每10周期打印一次）--------------------------
void print_motor_status(int motor_idx) {
    // 静默检查，不打印警告
    if (motor_configs[motor_idx].outputs == NULL || 
        motor_configs[motor_idx].inputs == NULL) {
        return; // 静默返回，不打印任何内容
    }
    
    inputs_t *in_ptr = (inputs_t *)motor_configs[motor_idx].inputs;
    uint16_t status = in_ptr->statusword;
    
    // 解析状态字关键位
    int ready_switch = (status & STATUSWORD_READY_SWITCH) ? 1 : 0;
    int switch_on = (status & STATUSWORD_SWITCH_ON) ? 1 : 0;
    int op_enable = (status & STATUSWORD_OP_ENABLE) ? 1 : 0;
    int fault = (status & STATUSWORD_FAULT) ? 1 : 0;
    int mode_ready = (status & STATUSWORD_MODE_READY) ? 1 : 0;
    
    // 计算弧度值（使用该电机的分辨率）
    double current_rad = position_to_radians(motor_configs[motor_idx].actual_pos, motor_configs[motor_idx].resolution);
    double target_rad = position_to_radians(motor_configs[motor_idx].target_pos, motor_configs[motor_idx].resolution);
    
    INFO_PRINT("%s:Pos=%6d(%.3frad) St=0x%04X(RS%dS%dO%dF%dM%d) | ", 
           motor_configs[motor_idx].axis_name,
           motor_configs[motor_idx].actual_pos, current_rad,
           status,
           ready_switch, switch_on, op_enable, fault, mode_ready);
}

// -------------------------- CSP控制主逻辑（初始化SOEM+从站状态切换）--------------------------
void csp_control_main(char *ifname) {
    int i, chk;

    // Step 1: 初始化EtherCAT主站
    INFO_PRINT("\nStep 1: 初始化EtherCAT主站（网卡: %s）...\n", ifname);
    if (!ecx_init(&ecx_context, ifname)) {
        INFO_PRINT("ERROR: 主站初始化失败！请用root权限运行\n");
        goto cleanup;
    }

    // Step 2: 配置从站（扫描总线）
    INFO_PRINT("Step 2: 扫描并配置从站...\n");
    int slave_count = ecx_config_init(&ecx_context);
    if (slave_count <= 0) {
        INFO_PRINT("ERROR: 总线上未发现从站！\n");
        goto cleanup;
    }
    INFO_PRINT("INFO: 发现 %d 个EtherCAT从站\n", slave_count);

    // Step 3: PDO映射
    INFO_PRINT("Step 3: 配置PDO映射...\n");
    ecx_context.manualstatechange = 1;
    // 切换所有从站到PRE_OP状态（PDO映射需此状态）
    ecx_context.slavelist[0].state = EC_STATE_PRE_OP;
    ecx_writestate(&ecx_context, 0);
    ecx_statecheck(&ecx_context, 0, EC_STATE_PRE_OP, EC_TIMEOUTSTATE * 4);
    if (ecx_context.slavelist[0].state != EC_STATE_PRE_OP) {
        INFO_PRINT("ERROR: 从站无法进入PRE_OP状态，PDO映射失败！\n");
        goto cleanup;
    }
    // 执行PDO映射
    if (ecx_config_map_group(&ecx_context, IOmap, 0) <= 0) {
        INFO_PRINT("ERROR: PDO映射失败！\n");
        goto cleanup;
    }
    INFO_PRINT("INFO: PDO映射成功 - 输出: %d字节 | 输入: %d字节\n",
           ecx_context.slavelist[0].Obytes, ecx_context.slavelist[0].Ibytes);
    detailed_pdo_analysis(); // 打印PDO详细信息（调试用）

    // Step 4: 配置DC同步（周期2ms）
    INFO_PRINT("Step 4: 配置DC同步...\n");
    ecx_configdc(&ecx_context);
    for (i = 1; i <= slave_count;i++){
        ecx_dcsync0(&ecx_context, i, 1, EC_CYCLETIME, 0);
    }

    INFO_PRINT("INFO: DC同步激活成功，周期: %d us\n", EC_CYCLETIME_US);

    // Step 5: 初始化电机配置（SDO+CSP+PDO映射）
    INFO_PRINT("Step 5: 初始化电机配置...\n");
    init_motor_configs();
    if (motor_count == 0) {
        INFO_PRINT("ERROR: 未配置任何电机！\n");
        goto cleanup;
    }

    // Step 5.5: 验证电机初始状态
    INFO_PRINT("Step 5.5: 验证电机初始状态...\n");
    for (int i = 0; i < motor_count; i++) {
        motor_config_t *motor = &motor_configs[i];
        inputs_t *in_ptr = (inputs_t *)motor->inputs;
        
        if (in_ptr) {
            double current_rad = position_to_radians(in_ptr->actual_pos, motor->resolution);
            INFO_PRINT("  %s (Motor %d) 初始状态: 状态字=0x%04X 模式=0x%02X 位置=%d(%.6frad) 错误码=0x%04X 分辨率=%d\n", 
                   motor->axis_name, i, in_ptr->statusword, in_ptr->mode_op, in_ptr->actual_pos, current_rad, in_ptr->error_code, motor->resolution);
        } else {
            INFO_PRINT("  %s (Motor %d) 初始状态: PDO指针无效\n", motor->axis_name, i);
        }
    }

    // Step 6: 切换从站到SAFE_OP状态
    INFO_PRINT("Step 6: 切换从站到SAFE_OP状态...\n");
    ecx_context.slavelist[0].state = EC_STATE_SAFE_OP;
    ecx_writestate(&ecx_context, 0);
    ecx_statecheck(&ecx_context, 0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE * 4);
    if (ecx_context.slavelist[0].state != EC_STATE_SAFE_OP) {
        INFO_PRINT("ERROR: 从站无法进入SAFE_OP状态！\n");
        for (i = 1; i <= slave_count; i++) {
            if (ecx_context.slavelist[i].state != EC_STATE_SAFE_OP) {
                INFO_PRINT("  %s (Slave %d): 状态=0x%02X | AL错误码=0x%04X\n",
                       motor_configs[i-1].axis_name, i, ecx_context.slavelist[i].state, ecx_context.slavelist[i].ALstatuscode);
            }
        }
        goto cleanup;
    }
    INFO_PRINT("INFO: 所有从站进入SAFE_OP状态！\n");

    // Step 7: 切换从站到OP状态，开始发送CSP指令
    INFO_PRINT("Step 7: 切换从站到OPERATIONAL状态...\n");
    expectedWKC = (ecx_context.grouplist[0].outputsWKC * 2) + ecx_context.grouplist[0].inputsWKC;
    INFO_PRINT("INFO: 预期工作计数器（WKC）: %d\n", expectedWKC);
    ecx_context.slavelist[0].state = EC_STATE_OPERATIONAL;
    ecx_writestate(&ecx_context, 0);
    // 等待从站切换到OP状态
    chk = 200;
    do {
        ecx_send_processdata(&ecx_context);
        wkc = ecx_receive_processdata(&ecx_context, EC_TIMEOUTRET);
        ecx_statecheck(&ecx_context, 0, EC_STATE_OPERATIONAL, EC_CYCLETIME_US);
    } while (chk-- && (ecx_context.slavelist[0].state != EC_STATE_OPERATIONAL) && !exit_flag);
    // 检查切换结果
    if (ecx_context.slavelist[0].state != EC_STATE_OPERATIONAL) {
        INFO_PRINT("WARNING: 从站未进入OP状态，仍尝试发送CSP指令...\n");
    } else {
        INFO_PRINT("INFO: 所有从站进入OPERATIONAL状态，开始发送CSP指令！\n");
    }
    inOP = TRUE;
    for (i = 0; i < motor_count; i++) {
        motor_configs[i].is_operational = TRUE;
    }

    for (i = 0 ;i < 10 ;i++){
        ecx_send_processdata(&ecx_context);
        wkc = ecx_receive_processdata(&ecx_context, EC_TIMEOUT);
        osal_usleep(EC_CYCLETIME_US);
    }

    // Step 8: 主循环（持续发送CSP指令）
    INFO_PRINT("\n===================== 开始CSP指令发送（Ctrl+C退出） =====================\n");
    
    int global_cycle = 0;
    while (!exit_flag) {
        global_cycle++;

        // 控制所有电机
        for (i = 0; i < motor_count; i++) {
            control_single_motor(i);
        }

        // 发送+接收PDO数据
        ecx_send_processdata(&ecx_context);
        wkc = ecx_receive_processdata(&ecx_context, EC_TIMEOUTRET);

        // 每10周期打印简版状态
        if (global_cycle % 10 == 0) {
            INFO_PRINT("全局周期: %4d | WKC: %d | ", global_cycle, wkc);
            for (i = 0; i < motor_count; i++) {
                print_motor_status(i);
            }
            INFO_PRINT("\r");
            fflush(stdout);
            needlf = TRUE;
        }

        // 每100周期打印PDO RawData（调试用）
        if (global_cycle % 100 == 0) {
            print_ecat_rawdata_pdo(global_cycle);
        }

        // 等待下一个周期（2ms）
        osal_usleep(EC_CYCLETIME_US);
    }

// 退出清理
cleanup:
    INFO_PRINT("\nStep 8: 退出清理，切换从站到INIT状态...\n");
    inOP = FALSE;
    if (ecx_context.slavelist != NULL) {
        ecx_context.slavelist[0].state = EC_STATE_INIT;
        ecx_writestate(&ecx_context, 0);
        ecx_statecheck(&ecx_context, 0, EC_STATE_INIT, EC_TIMEOUTSTATE * 2);
        ecx_close(&ecx_context);
    }
}

// -------------------------- 辅助函数（用法提示+信号处理）--------------------------
void print_usage() {
    INFO_PRINT("用法: ./multi_motor_csp <以太网适配器名称>\n");
    INFO_PRINT("示例: ./multi_motor_csp enp2s0\n");
    INFO_PRINT("注意: 必须用root权限运行（访问网卡需要）\n");
}

void signal_handler(int sig) {
    if (sig == SIGINT) {
        INFO_PRINT("\n\nINFO: 收到Ctrl+C，准备退出...\n");
        exit_flag = TRUE;
    }
}